#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#一条规划，回到middle位置再送食物，会掉电，所以延时后回到middle
import sys
import rospy
import moveit_commander
import geometry_msgs.msg
from moveit_commander import RobotTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint


def scale_trajectory_speed(traj, scale):
       
       # Create a new trajectory object
       new_traj = RobotTrajectory()
       
       # Initialize the new trajectory to be the same as the input trajectory
       new_traj.joint_trajectory = traj.joint_trajectory
 
       # Get the number of joints involved
       n_joints = len(traj.joint_trajectory.joint_names)
       
       # Get the number of points on the trajectory
       n_points = len(traj.joint_trajectory.points)
        
       # Store the trajectory points
       points = list(traj.joint_trajectory.points)
       
       # Cycle through all points and joints and scale the time from start,
       # as well as joint speed and acceleration
       for i in range(n_points):
           
           point = JointTrajectoryPoint()
           
           # The joint positions are not scaled so pull them out first
           point.positions = traj.joint_trajectory.points[i].positions

           # Next, scale the time_from_start for this point
           point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
           
           # Get the joint velocities for this point
           point.velocities = list(traj.joint_trajectory.points[i].velocities)
           
           # Get the joint accelerations for this point
           point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
           
           # Scale the velocity and acceleration for each joint at this point
           for j in range(n_joints):
               point.velocities[j] = point.velocities[j] * scale
               point.accelerations[j] = point.accelerations[j] * scale * scale
        
           # Store the scaled trajectory point
           points[i] = point

       # Assign the modified points to the new trajectory
       new_traj.joint_trajectory.points = points

       # Return the new trajecotry
       return new_traj

def execute_tra1():
    # 初始化 moveit_commander 和 rospy
    moveit_commander.roscpp_initialize(sys.argv)

    # 初始化机器人手臂的 move group
    arm_group = moveit_commander.MoveGroupCommander("manipulator")

    # 获取初始位置
    arm_group.set_named_target("middle")
    plan = arm_group.go(wait=True)

    # 清空之前的目标
    arm_group.clear_pose_targets()

    # 定义多个目标位姿
    waypoints = []

    # 第1个目标位姿 0/-55/90/20/0
    # 第1个目标位姿 30/-70/90/-20/0
    pose1 = geometry_msgs.msg.Pose()
    pose1.position.x = -0.0011310404075417224
    pose1.position.y = 0.06907060201161805
    pose1.position.z = 0.3073585902967819
    pose1.orientation.x = 2.688964454397768e-05
    pose1.orientation.y = -9.329175674129769e-05
    pose1.orientation.z = 0.2588310042911079
    pose1.orientation.w = 0.9659226168753127
    waypoints.append(pose1)

    # 第2个目标位姿 35/-55/110/-10/0
    pose2 = geometry_msgs.msg.Pose()
    pose2.position.x = 0.009665201991908984
    pose2.position.y = 0.09130426767319212
    pose2.position.z = 0.11498378100534505
    pose2.orientation.x = -0.11512122029744272
    pose2.orientation.y = 0.36498810976129026
    pose2.orientation.z = 0.27778707139570713
    pose2.orientation.w = 0.8811158421771251
    waypoints.append(pose2)

    # 第3个目标位姿 35/-64/120/-13/0
    pose3 = geometry_msgs.msg.Pose()
    pose3.position.x = 0.004645320998532015
    pose3.position.y = 0.08780303492397415
    pose3.position.z = 0.09358932776876472
    pose3.orientation.x = -0.11023725592191891
    pose3.orientation.y = 0.3495295074860454
    pose3.orientation.z = 0.2797869790589822
    pose3.orientation.w = 0.8873534341807761
    waypoints.append(pose3)

    # 4 35/-60/115/-10/0 有点高
    pose4 = geometry_msgs.msg.Pose()
    pose4.position.x = 0.002689802791981763
    pose4.position.y = 0.0864335660110534
    pose4.position.z = 0.1016529166473485
    pose4.orientation.x = -0.11515741145952224
    pose4.orientation.y = 0.3650111716795012
    pose4.orientation.z = 0.27781956196470253
    pose4.orientation.w = 0.8810913154292472
    waypoints.append(pose4)

    # 5 35/-55/125/-30/0
    pose5 = geometry_msgs.msg.Pose()
    pose5.position.x = 0.040521254025108394
    pose5.position.y = 0.11294248356505611
    pose5.position.z = 0.08770454646571219
    pose5.orientation.x = -0.10292980365209553
    pose5.orientation.y = 0.3261289152765809
    pose5.orientation.z = 0.2825969968796311
    pose5.orientation.w = 0.8962055140955537
    waypoints.append(pose5)

    # 第6个目标位姿 35/-50/122/-32/0
    pose6 = geometry_msgs.msg.Pose()
    pose6.position.x = 0.05010657634938402
    pose6.position.y = 0.1196340400624652
    pose6.position.z = 0.0942325579027638
    pose6.orientation.x = -0.10295037417731849
    pose6.orientation.y = 0.32615192764866546
    pose6.orientation.z = 0.28254401566504306
    pose6.orientation.w = 0.8962114816044716
    waypoints.append(pose6)

    # 第7个目标位姿 32/-45/120/-35/0
    pose7 = geometry_msgs.msg.Pose()
    pose7.position.x = 0.06747123197664025
    pose7.position.y = 0.11753887303608636
    pose7.position.z = 0.09779214944642986
    pose7.orientation.x = -0.09434732313454879
    pose7.orientation.y = 0.3287544696671605
    pose7.orientation.z = 0.25884543602305454
    pose7.orientation.w = 0.9033372136368811
    waypoints.append(pose7)

    # 第8个目标位姿 32/-58/138/-60/0
    pose8 = geometry_msgs.msg.Pose()
    pose8.position.x = 0.0904501718573451
    pose8.position.y = 0.13184367830868512
    pose8.position.z = 0.11971295004270945
    pose8.orientation.x = -0.047902301026064985
    pose8.orientation.y = 0.16691573525552947
    pose8.orientation.z = 0.2711677829418468
    pose8.orientation.w = 0.9467378414191111
    waypoints.append(pose8)

    # 第9个目标位姿 -32/-70/150/-78/0 举平
    pose9 = geometry_msgs.msg.Pose()
    pose9.position.x = 0.09432316993420176
    pose9.position.y = 0.13433610299295476
    pose9.position.z = 0.16009946275824632
    pose9.orientation.x = -0.004805209403384029
    pose9.orientation.y = 0.016742670630141412
    pose9.orientation.z = 0.27542811607470175
    pose9.orientation.w = 0.9611638496210213
    waypoints.append(pose9)


    # 第10个目标位姿 -20/-70/150/-78/0 举平移
    pose10 = geometry_msgs.msg.Pose()
    pose10.position.x = 0.11755533690651693
    pose10.position.y = 0.08668575883939508
    pose10.position.z = 0.16011078889265554
    pose10.orientation.x = -0.00302493770022203
    pose10.orientation.y = 0.017144256975920424
    pose10.orientation.z = 0.17347487077387133
    pose10.orientation.w = 0.9846844131063731
    waypoints.append(pose10)

    # # 第11个目标位姿 
    pose11 = geometry_msgs.msg.Pose()
    pose11.position.x = 0.009070536838216042
    pose11.position.y = 0.04719269375993429
    pose11.position.z = 0.3072916016195043
    pose11.orientation.x = -1.1967649283791688e-06
    pose11.orientation.y = 1.7336762845100254e-05
    pose11.orientation.z = 0.1735087311185056
    pose11.orientation.w = 0.9848323308683823
    waypoints.append(pose11)

    # 第14个目标位姿 0/-70/90/-20 设定这个位置为初始位置
    pose14 = geometry_msgs.msg.Pose()
    pose14.position.x = 0.017405918539271842
    pose14.position.y = -1.2171559032328237e-06
    pose14.position.z = 0.30727696933440546
    pose14.orientation.x = 1.829736299180823e-06
    pose14.orientation.y = 8.08447651258564e-05
    pose14.orientation.z = 6.071146226902279e-06
    pose14.orientation.w = 0.9999999967119587
    waypoints.append(pose14)

    # 第15个目标位姿 送食位置
    pose15 = geometry_msgs.msg.Pose()
    pose15.position.x = 0.3578591546734353
    pose15.position.y = -4.5874290626404854e-05
    pose15.position.z = 0.38613742059913403
    pose15.orientation.x = -4.9109689661127625e-06
    pose15.orientation.y = -0.06983318184000316
    pose15.orientation.z = -2.5349309214965662e-05
    pose15.orientation.w = 0.9975586830093738
    waypoints.append(pose15)



    # 规划并执行笛卡尔轨迹
    (plan, fraction) = arm_group.compute_cartesian_path(
        waypoints,   # 目标位姿序列
        0.01,        # eef_step，终端执行器的步长
        False)         # jump_threshold，跳跃阈值
    print("Fraction of path achieved: ", fraction)

    # 执行轨迹
    new_plan=scale_trajectory_speed(plan,0.7)
    arm_group.execute(new_plan, wait=True)

    # 停止运动
    arm_group.stop()

    # 清空之前的目标
    arm_group.clear_pose_targets()
    
    rospy.sleep(3)
    arm_group.set_named_target("middle")
    plan = arm_group.go(wait=True)

    # 关闭并退出 moveit_commander
    moveit_commander.roscpp_shutdown()
    moveit_commander.os._exit(0)

def execute_tra2():
    # 初始化 moveit_commander 和 rospy
    moveit_commander.roscpp_initialize(sys.argv)

    # 初始化机器人手臂的 move group
    arm_group = moveit_commander.MoveGroupCommander("manipulator")

    # 获取初始位置
    arm_group.set_named_target("middle")
    plan = arm_group.go(wait=True)

    # 清空之前的目标
    arm_group.clear_pose_targets()

    # 定义多个目标位姿
    waypoints = []

    # 第1个目标位姿 0/-55/90/20/0
    pose1 = geometry_msgs.msg.Pose()
    pose1.position.x = -0.017099604521405784
    pose1.position.y = -2.8315387428728567e-05
    pose1.position.z = 0.11438909247802503
    pose1.orientation.x = -3.453025354057534e-05
    pose1.orientation.y = 0.46175405203633646
    pose1.orientation.z = -7.30327633778306e-05
    pose1.orientation.w = 0.887007998217548
    waypoints.append(pose1)

    # 第2个目标位姿 0/-58/115/-3/0
    pose2 = geometry_msgs.msg.Pose()
    pose2.position.x = 0.004651029454941641
    pose2.position.y = -2.046464646042786e-06
    pose2.position.z = 0.07989258904926122
    pose2.orientation.x = -3.9551522751682376e-05
    pose2.orientation.y = 0.4523160885130879
    pose2.orientation.z = 6.4712743132175465e-06
    pose2.orientation.w = 0.8918576985517478
    waypoints.append(pose2)

    # 第3个目标位姿 0/-80/131/-11/0
    pose3 = geometry_msgs.msg.Pose()
    pose3.position.x = 0.016710764091943137
    pose3.position.y = 4.7481613370422367e-05
    pose3.position.z = 0.06725314521298217
    pose3.orientation.x = 2.484310933627909e-06
    pose3.orientation.y = 0.34210428530462056
    pose3.orientation.z = 0.00014130528033512744
    pose3.orientation.w = 0.9396619807158642
    waypoints.append(pose3)

    # 4 0/-90/142/-26/0
    pose4 = geometry_msgs.msg.Pose()
    pose4.position.x = 0.061313835276576675
    pose4.position.y = 3.4229349294604146e-05
    pose4.position.z = 0.09607582904327464
    pose4.orientation.x = 8.189114554395947e-06
    pose4.orientation.y = 0.19079166522506064
    pose4.orientation.z = 9.397613957952172e-05
    pose4.orientation.w = 0.9816305473891067
    waypoints.append(pose4)

    # 5 0/-90/146/-32/0
    pose5 = geometry_msgs.msg.Pose()
    pose5.position.x = 0.06469858300599692
    pose5.position.y = 6.72000076315633e-05
    pose5.position.z = 0.07814980502181258
    pose5.orientation.x = -3.84250569528825e-06
    pose5.orientation.y = 0.20806965601170563
    pose5.orientation.z = 0.00018064511789783727
    pose5.orientation.w = 0.978113994174374
    waypoints.append(pose5)

    # # 第6个目标位姿 0/-68/142/-62/0 举平
    # pose6 = geometry_msgs.msg.Pose()
    # pose6.position.x = 0.12194636235398673
    # pose6.position.y = -0.0002343643231762216
    # pose6.position.z = 0.14228208314683233
    # pose6.orientation.x = -0.0005234515020643942
    # pose6.orientation.y = 0.10451258921853264
    # pose6.orientation.z = -0.0013561753209920613
    # pose6.orientation.w = 0.9945225012446235
    # waypoints.append(pose6)

    # 第7个目标位姿 0/-90/155/-65/0 举平new
    pose7 = geometry_msgs.msg.Pose()
    pose7.position.x = 0.10862679372272958
    pose7.position.y = 7.322812585541887e-05
    pose7.position.z = 0.151673237887705
    pose7.orientation.x = 1.843833939518914e-06
    pose7.orientation.y = 0.0002691494452088297
    pose7.orientation.z = 0.00018408295291220934
    pose7.orientation.w = 0.9999999468343201
    waypoints.append(pose7)

    # 第8个目标位姿 -15/-90/155/-68/0
    pose8 = geometry_msgs.msg.Pose()
    pose8.position.x = 0.10150451167124033
    pose8.position.y = -0.059668682905588945
    pose8.position.z = 0.1647479924791181 #0.15
    pose8.orientation.x = -0.003413749283397351
    pose8.orientation.y = -0.02598662685755732
    pose8.orientation.z = -0.1307826110962234
    pose8.orientation.w = 0.9910645539899258
    waypoints.append(pose8)

    # # 第9个目标位姿 -15/-58/140/-82/0
    # pose9 = geometry_msgs.msg.Pose()
    # pose9.position.x = 0.1008246526008469
    # pose9.position.y = -0.05933329930388463
    # pose9.position.z = 0.23562737676564936
    # pose9.orientation.x = 4.972686461035324e-05
    # pose9.orientation.y = 0.00036333183624753817
    # pose9.orientation.z = -0.1304233799329774
    # pose9.orientation.w = 0.9914583236243842
    # waypoints.append(pose9)


    # 第10个目标位姿 
    pose10 = geometry_msgs.msg.Pose()
    pose10.position.x = 0.12119300384217499
    pose10.position.y = -0.06496064015101635
    pose10.position.z = 0.16648646107426138
    pose10.orientation.x = -0.003406584361246141
    pose10.orientation.y = -0.02590040792578116
    pose10.orientation.z = -0.130787996023333
    pose10.orientation.w = 0.9910661250130935
    waypoints.append(pose10)

    # 第11个目标位姿 
    pose11 = geometry_msgs.msg.Pose()
    pose11.position.x = 0.0943541206120134
    pose11.position.y = -0.05774831859841958
    pose11.position.z = 0.2898386501669852
    pose11.orientation.x = -0.020411665320870926
    pose11.orientation.y = -0.1551294495136764
    pose11.orientation.z = -0.12918976954612996
    pose11.orientation.w = 0.9791977436948222
    waypoints.append(pose11)

    # 第12个目标位姿 
    pose12 = geometry_msgs.msg.Pose()
    pose12.position.x = 0.09961796537357243
    pose12.position.y = -0.05914946048063186
    pose12.position.z = 0.2557615185418525
    pose12.orientation.x = -0.011369493862450104
    pose12.orientation.y = -0.0864123554335495
    pose12.orientation.z = -0.13027786365359298
    pose12.orientation.w = 0.9876392649543626
    waypoints.append(pose11)

    # 第12个目标位姿 
    pose13 = geometry_msgs.msg.Pose()
    pose13.position.x = 0.10198085464213251
    pose13.position.y = -6.745282878304773e-05
    pose13.position.z = 0.2898271081231947
    pose13.orientation.x = 1.7551940811496022e-05
    pose13.orientation.y = -0.1564288209154345
    pose13.orientation.z = -0.00017877032414685562
    pose13.orientation.w = 0.9876892181856132
    waypoints.append(pose13)

    # 第14个目标位姿 0/-70/90/-20 设定这个位置为初始位置
    pose14 = geometry_msgs.msg.Pose()
    pose14.position.x = 0.017405918539271842
    pose14.position.y = -1.2171559032328237e-06
    pose14.position.z = 0.30727696933440546
    pose14.orientation.x = 1.829736299180823e-06
    pose14.orientation.y = 8.08447651258564e-05
    pose14.orientation.z = 6.071146226902279e-06
    pose14.orientation.w = 0.9999999967119587
    waypoints.append(pose14)

    # 第15个目标位姿 送食位置
    pose15 = geometry_msgs.msg.Pose()
    pose15.position.x = 0.3578591546734353
    pose15.position.y = -4.5874290626404854e-05
    pose15.position.z = 0.38613742059913403
    pose15.orientation.x = -4.9109689661127625e-06
    pose15.orientation.y = -0.06983318184000316
    pose15.orientation.z = -2.5349309214965662e-05
    pose15.orientation.w = 0.9975586830093738
    waypoints.append(pose15)



    # 规划并执行笛卡尔轨迹
    (plan, fraction) = arm_group.compute_cartesian_path(
        waypoints,   # 目标位姿序列
        0.01,        # eef_step，终端执行器的步长
        False)         # jump_threshold，跳跃阈值
    print("Fraction of path achieved: ", fraction)

    # 执行轨迹
    new_plan=scale_trajectory_speed(plan,0.7)
    arm_group.execute(new_plan, wait=True)

    # 停止运动
    arm_group.stop()

    # 清空之前的目标
    arm_group.clear_pose_targets()
    
    rospy.sleep(3)
    arm_group.set_named_target("middle")
    plan = arm_group.go(wait=True)

    # 关闭并退出 moveit_commander
    moveit_commander.roscpp_shutdown()
    moveit_commander.os._exit(0)

def execute_tra3():
    # 初始化 moveit_commander 和 rospy
    moveit_commander.roscpp_initialize(sys.argv)

    # 初始化机器人手臂的 move group
    arm_group = moveit_commander.MoveGroupCommander("manipulator")

    # 获取初始位置
    arm_group.set_named_target("middle")
    plan = arm_group.go(wait=True)

    # 清空之前的目标
    arm_group.clear_pose_targets()

    # 定义多个目标位姿
    waypoints = []

    # 第1个目标位姿 -30/-70/90/-20/0
    pose1 = geometry_msgs.msg.Pose()
    pose1.position.x = -0.0011302048333630965
    pose1.position.y = -0.06905961719492143
    pose1.position.z = 0.3073550976191969
    pose1.orientation.x = -1.7316002977395957e-05
    pose1.orientation.y = -7.170005905241826e-05
    pose1.orientation.z = -0.2588197909540237
    pose1.orientation.w = 0.9659256236221158
    waypoints.append(pose1)

    # 第2个目标位姿 -35/-55/110/-10/0
    pose2 = geometry_msgs.msg.Pose()
    pose2.position.x = 0.009670549573942186
    pose2.position.y = -0.09129423315793453
    pose2.position.z = 0.11500108680844905
    pose2.orientation.x = 0.1150590441241054
    pose2.orientation.y = 0.3649594495490963
    pose2.orientation.z = -0.2777586943432337
    pose2.orientation.w = 0.881144780536553
    waypoints.append(pose2)

    # 4 -35/-60/115/-10/0 
    pose4 = geometry_msgs.msg.Pose()
    pose4.position.x = 0.002689802791981763
    pose4.position.y = -0.08642607255938041
    pose4.position.z = 0.10169023602364938
    pose4.orientation.x = 0.11506762902098444
    pose4.orientation.y = 0.3649312412859769
    pose4.orientation.z = -0.2777332746713525
    pose4.orientation.w = 0.8811633549037843
    waypoints.append(pose4)

    # 5 -35/-55/125/-30/0
    pose5 = geometry_msgs.msg.Pose()
    pose5.position.x = 0.04053697379986093
    pose5.position.y = -0.11290902734774333
    pose5.position.z = 0.0876834203451664
    pose5.orientation.x = 0.10283628094347873
    pose5.orientation.y = 0.32616481198444003
    pose5.orientation.z = -0.2825256471019672
    pose5.orientation.w = 0.8962256822221084
    waypoints.append(pose5)

    # 第6个目标位姿 -35/-50/122/-32/0
    pose6 = geometry_msgs.msg.Pose()
    pose6.position.x = 0.05010120615902845
    pose6.position.y = -0.11958529726583864
    pose6.position.z = 0.09421890307418768
    pose6.orientation.x = 0.1028380818468409
    pose6.orientation.y = 0.3262024609161537
    pose6.orientation.z = -0.28248616522541353
    pose6.orientation.w = 0.8962242185248891
    waypoints.append(pose6)

    # 第7个目标位姿 -32/-45/120/-35/0
    pose7 = geometry_msgs.msg.Pose()
    pose7.position.x = 0.06745494524328567
    pose7.position.y = -0.11761062296318618
    pose7.position.z = 0.09681908582144214
    pose7.orientation.x = 0.09426878785030171
    pose7.orientation.y = 0.32868644227651406
    pose7.orientation.z = -0.259031952013786
    pose7.orientation.w = 0.9033167031206555
    waypoints.append(pose7)

    # 第8个目标位姿 32/-58/138/-60/0
    pose8 = geometry_msgs.msg.Pose()
    pose8.position.x = 0.09038448867848176
    pose8.position.y = -0.1319360201480157
    pose8.position.z = 0.1197025357922735
    pose8.orientation.x = 0.04787093957528583
    pose8.orientation.y = 0.16692700118128137
    pose8.orientation.z = -0.2714553558001961
    pose8.orientation.w = 0.9466550265160975
    waypoints.append(pose8)

    # 第9个目标位姿 -32/-70/150/-78/0 举平
    pose9 = geometry_msgs.msg.Pose()
    pose9.position.x = 0.0942933762027911
    pose9.position.y = -0.13439538389383587
    pose9.position.z = 0.16010801190356336
    pose9.orientation.x = 0.004799855100987425
    pose9.orientation.y = 0.0167298817281641
    pose9.orientation.z = -0.27562936709118274
    pose9.orientation.w = 0.9611064064115302
    waypoints.append(pose9)


    # 第10个目标位姿 -20/-70/150/-78/0 举平移
    pose10 = geometry_msgs.msg.Pose()
    pose10.position.x = 0.11754935088930349
    pose10.position.y = -0.08673064743303559
    pose10.position.z = 0.1601478830358012
    pose10.orientation.x = 0.0030148054312412937
    pose10.orientation.y = 0.017086839017522924
    pose10.orientation.z = -0.17361459296382126
    pose10.orientation.w = 0.984660816723509
    waypoints.append(pose10)

    # # 第11个目标位姿 -20/-70/90/-20/0
    pose11 = geometry_msgs.msg.Pose()
    pose11.position.x = 0.009071977232847944
    pose11.position.y = -0.047260477105669295
    pose11.position.z = 0.3073157690252368
    pose11.orientation.x = 3.856803300718398e-06
    pose11.orientation.y = 1.1296182120014837e-05
    pose11.orientation.z = -0.17367575774823565
    pose11.orientation.w = 0.9848028894291982
    waypoints.append(pose11)

    # 第14个目标位姿 0/-70/90/-20 设定这个位置为初始位置
    pose14 = geometry_msgs.msg.Pose()
    pose14.position.x = 0.017405918539271842
    pose14.position.y = -1.2171559032328237e-06
    pose14.position.z = 0.30727696933440546
    pose14.orientation.x = 1.829736299180823e-06
    pose14.orientation.y = 8.08447651258564e-05
    pose14.orientation.z = 6.071146226902279e-06
    pose14.orientation.w = 0.9999999967119587
    waypoints.append(pose14)

    # 第15个目标位姿 送食位置
    pose15 = geometry_msgs.msg.Pose()
    pose15.position.x = 0.3578591546734353
    pose15.position.y = -4.5874290626404854e-05
    pose15.position.z = 0.38613742059913403
    pose15.orientation.x = -4.9109689661127625e-06
    pose15.orientation.y = -0.06983318184000316
    pose15.orientation.z = -2.5349309214965662e-05
    pose15.orientation.w = 0.9975586830093738
    waypoints.append(pose15)



    # 规划并执行笛卡尔轨迹
    (plan, fraction) = arm_group.compute_cartesian_path(
        waypoints,   # 目标位姿序列
        0.01,        # eef_step，终端执行器的步长
        False)         # jump_threshold，跳跃阈值
    print("Fraction of path achieved: ", fraction)

    # 执行轨迹
    new_plan=scale_trajectory_speed(plan,0.7)
    arm_group.execute(new_plan, wait=True)

    # 停止运动
    arm_group.stop()

    # 清空之前的目标
    arm_group.clear_pose_targets()
    
    rospy.sleep(3)
    arm_group.set_named_target("middle")
    plan = arm_group.go(wait=True)

    # 关闭并退出 moveit_commander
    moveit_commander.roscpp_shutdown()
    moveit_commander.os._exit(0)
